#include "KdTree.h"
#include <algorithm>	// nth_element

//=============================================================================
// KdTree
//	Default constructor
//=============================================================================
KdTree::KdTree()
{
	m_MAX_TREE_DEPTH = 0;
}

//=============================================================================
// KD_NODE_t
//	Default constructor
//=============================================================================
KdTree::KD_NODE_t::KD_NODE_t ()
{
	SOLE_MEDIAN_VALUE = 0.0f;

	DIVIDES_BY = none;
	BRANCH_WAY = root;

	MEDIAN_CENTROID = XMFLOAT3(0.0f, 0.0f, 0.0f);
	BB_MIN = XMFLOAT3(0.0f, 0.0f, 0.0f);
	BB_MAX = XMFLOAT3(0.0f, 0.0f, 0.0f);
	SPLITTING_PLANE[0] = XMFLOAT3(0.0f, 0.0f, 0.0f);
	SPLITTING_PLANE[1] = XMFLOAT3(0.0f, 0.0f, 0.0f);
	SPLITTING_PLANE[2] = XMFLOAT3(0.0f, 0.0f, 0.0f);
	SPLITTING_PLANE[3] = XMFLOAT3(0.0f, 0.0f, 0.0f);

	LEFT_CHILD_ptr = nullptr;
	RIGHT_CHILD_ptr = nullptr;
}


//=============================================================================
// MakeTree_r
//	This recursive method creates the kd-tree. This recursive method has the
//	neat sideeffect of sorting the binary tree from lowest to highest triangle
//	values. The triangles are stored inside the leaf nodes.
//
// PSEUDO CODE:
//	1)	Create new node.
//	2)	Set node branch (root, left or right)
//	3)	if ( depth is not 0 )
//		a)	Calculate subtrees boundry box from previous node.
//		b)	Choose splitting dimension.
//	4)	else
//		a)	Calculate subtrees boundry box from all triangles since this node
//			is the root node.
//		b)	Choose splitting dimension.
//	5)	if ( amount of remaining triangles <= 0 )
//		a)	Safety measure.
//			Delete new node and return a null pointer. This code should never
//			be reached!
//	6)	if ( maximum depth is reached || amount of remaining triangles == 1 )
//		a)	Leaf node.
//			Store all remaining triangles from the given subtree triangle list.
//		b)	Check if other triangles from outside the given subtree triangle
//			list are intersecting with this node and store them as well.
//	7) else
//		a)	Get median with: current idx + (amount of remaining triangles / 2)
//		b)	Sort subtree list with nth_element method by use of median index
//			and a comparator for choosing the right dimension when comparing.
//		c)	Create splitting plane points from previous node and store them in
//			the new node.
//		d)	Store median value and centroid of median triangle.
//		e)	Access next recursion depth
//			-)	node->left  = MakeTree from idx with length = median-idx
//			-)	node->right = MakeTree from median with
//							  length = length + idx - median
//	8) return node
//=============================================================================
KdTree::KD_NODE_t* KdTree::MakeTree_r( vector<TriangleAccessor *> complete_list, KD_NODE_t *prev_node, int current_idx, int length, short split_dimension, int branch, int depth )
{	
	KD_NODE_t *newNode = new KD_NODE_t();

	// Set node branch
	if ( branch == 0 )
		newNode->BRANCH_WAY = KD_NODE_t::Branch::left;
	else if ( branch == 1 )
		newNode->BRANCH_WAY = KD_NODE_t::Branch::right;
	else
		newNode->BRANCH_WAY = KD_NODE_t::Branch::root;

	// Get subtree bounds and set split dimension
	if ( depth != 0 )
	{
		GetSubtreeBounds( newNode, prev_node );
		split_dimension = ChooseSplittingDimension( newNode  );
	}
	else
	{
		// In case it's the very first call, calculate everything from all triangles
		newNode->BB_MIN = GetLowerRootCentroidBound( complete_list );
		newNode->BB_MAX = GetUpperRootCentroidBound( complete_list );
		split_dimension = ChooseSplittingDimension( newNode  );
	}

	if ( length <= 0 )
	{
		delete newNode;
		return nullptr;	// safety measure
	}
	else if ( depth >= m_MAX_TREE_DEPTH || length == 1)
	{
		// Leaf node. Store data.
		newNode->DIVIDES_BY = KD_NODE_t::none;

		// Loop is actually a work around as the insert method for vector does
		// not work. Compiler reports "STORED_TRI_LIST_ptr" is not defined and
		// I don't know how to initialize a zero-sized vector of pointers.
		for ( int idx = current_idx; idx < current_idx+length; ++idx )
		{
			/*newNode->STORED_TRI_LIST_ptr.push_back( &(*(complete_list->begin()+idx)) );*/
			newNode->STORED_TRI_LIST_ptr.push_back( complete_list.at(idx) );
		}

		// Include remaining triangles
		if ( current_idx > 0 )
		{
			// ... from left side of tree
			for ( int idx = 0; idx < current_idx; ++idx )
			{
				// Check all three edge points of a triangle and terminate loop
				// if one is inside the splitting box. Store this triangle.
				for ( int tri_point = 0; tri_point < 3; ++tri_point )
				{
					if ( complete_list.at(idx)->GetRealPositions()[tri_point].x < newNode->BB_MAX.x &&
						 complete_list.at(idx)->GetRealPositions()[tri_point].y < newNode->BB_MAX.y &&
						 complete_list.at(idx)->GetRealPositions()[tri_point].z < newNode->BB_MAX.z )
					{
						if ( complete_list.at(idx)->GetRealPositions()[tri_point].x > newNode->BB_MIN.x &&
							 complete_list.at(idx)->GetRealPositions()[tri_point].y > newNode->BB_MIN.y &&
							 complete_list.at(idx)->GetRealPositions()[tri_point].z > newNode->BB_MIN.z )
						{
							newNode->STORED_TRI_LIST_ptr.push_back ( complete_list.at(idx) );
							tri_point = 3;
						}
					}
				}
			}
		}
		if ( (current_idx+length) < complete_list.size() )
		{
			// ... from right side of tree
			for ( int idx = current_idx+length; idx < complete_list.size(); ++idx )
			{
				for ( int tri_point = 0; tri_point < 3; ++tri_point )
				{
					if ( complete_list.at(idx)->GetRealPositions()[tri_point].x < newNode->BB_MAX.x &&
						 complete_list.at(idx)->GetRealPositions()[tri_point].y < newNode->BB_MAX.y &&
						 complete_list.at(idx)->GetRealPositions()[tri_point].z < newNode->BB_MAX.z )
					{
						if ( complete_list.at(idx)->GetRealPositions()[tri_point].x > newNode->BB_MIN.x &&
							 complete_list.at(idx)->GetRealPositions()[tri_point].y > newNode->BB_MIN.y &&
							 complete_list.at(idx)->GetRealPositions()[tri_point].z > newNode->BB_MIN.z )
						{
							newNode->STORED_TRI_LIST_ptr.push_back ( complete_list.at(idx) );
							tri_point = 3;
						}
					}
				}
			}
		}
	}
	else
	{
		// Calculate median index
		int median = current_idx + (length / 2.0f);

		// Sort list with nth_element with lower values before and highervalues after the one at median index
		if ( split_dimension == 0 )
		{
			newNode->DIVIDES_BY = KD_NODE_t::x;
			newNode->SOLE_MEDIAN_VALUE = complete_list.at(median)->GetCentroid().x;
			nth_element( complete_list[current_idx], complete_list[median], complete_list[current_idx+length-1], nth_element_comp_X );
		}
		else if ( split_dimension == 1 )
		{
			newNode->DIVIDES_BY = KD_NODE_t::y;
			newNode->SOLE_MEDIAN_VALUE = complete_list.at(median)->GetCentroid().x;
			nth_element( complete_list[current_idx], complete_list[median], complete_list[current_idx+length-1], nth_element_comp_Y );
		}
		else
		{
			newNode->DIVIDES_BY = KD_NODE_t::z;
			newNode->SOLE_MEDIAN_VALUE = complete_list.at(median)->GetCentroid().x;
			nth_element( complete_list[current_idx], complete_list[median], complete_list[current_idx+length-1], nth_element_comp_Z );
		}
			
		// Save median data
		newNode->MEDIAN_CENTROID = complete_list.at(median)->GetCentroid();

		// Create splitting plane points
		GetSplittingPlanePoints( newNode, median, split_dimension );

		// Save splitting points in LineVertex
		LineVertex vertex = {newNode->SPLITTING_PLANE[0], XMFLOAT3(0.f, 1.f, 1.f)};
		m_LINEVERTEX_LIST.push_back(vertex);
		vertex.Pos = newNode->SPLITTING_PLANE[1];
		m_LINEVERTEX_LIST.push_back(vertex);
		m_LINEVERTEX_LIST.push_back(vertex);
		vertex.Pos = newNode->SPLITTING_PLANE[2];
		m_LINEVERTEX_LIST.push_back(vertex);
		m_LINEVERTEX_LIST.push_back(vertex);
		vertex.Pos = newNode->SPLITTING_PLANE[3];
		m_LINEVERTEX_LIST.push_back(vertex);
		m_LINEVERTEX_LIST.push_back(vertex);
		vertex.Pos = newNode->SPLITTING_PLANE[0];	// back to first plane point
		m_LINEVERTEX_LIST.push_back(vertex);

		// Recursive call for left and right subtree
		newNode->LEFT_CHILD_ptr = MakeTree_r( complete_list, newNode, current_idx, median-current_idx, split_dimension, 0, depth+1 );
		newNode->RIGHT_CHILD_ptr = MakeTree_r( complete_list, newNode, median, length+current_idx-median, split_dimension, 1, depth+1 );
	}

	return newNode;
}

//=============================================================================
// Quicksort_r
//	This method quicksorts the given node list between the start and end node.
//	It sorts based on the current depth.
//=============================================================================
void KdTree::Quicksort_r( vector<TriangleAccessor *> triangleList, int startNode, int endNode, int lookupDimension )
{
	int i = startNode, j = endNode;
	int pivotIdx = startNode + ((endNode - startNode) / 2);
	float pivot = 0.0f;
	
	// Get pivot from current accessed dimension
	if ( lookupDimension == 0 )			pivot = triangleList[pivotIdx]->GetCentroid().x;
	else if ( lookupDimension == 1 )	pivot = triangleList[pivotIdx]->GetCentroid().y;
	else								pivot = triangleList[pivotIdx]->GetCentroid().z;

	while ( i <= j )
	{
		if ( lookupDimension == 0 )
		{
			while ( triangleList[i]->GetCentroid().x < pivot )
				i++;
			while ( triangleList[j]->GetCentroid().x > pivot )
				j--;
		}
		else if ( lookupDimension == 1 )
		{
			while ( triangleList[i]->GetCentroid().y < pivot )
				i++;
			while ( triangleList[j]->GetCentroid().y > pivot )
				j--;
		}
		else
		{
			while ( triangleList[i]->GetCentroid().z < pivot )
				i++;
			while ( triangleList[j]->GetCentroid().z > pivot )
				j--;
		}

		if (i <= j)
		{
			SwapValues( *triangleList[i], *triangleList[j] );
			i++;
			j--;
		}
	}

	/* Recursion */
	if (startNode < j)
		Quicksort_r( triangleList, startNode, j, lookupDimension );
	if (i < endNode)
		Quicksort_r( triangleList, i, endNode, lookupDimension );
}

//=============================================================================
// SwapValues
//	This method swaps two nodes for quicksort
//=============================================================================
void KdTree::SwapValues( TriangleAccessor &a, TriangleAccessor &b )
{
	TriangleAccessor tmp = a;
	a = b;
	b = tmp;
}

//=============================================================================
// DeleteTree_r
//	This method removes all nodes from the tree, the root node inclusive.
//=============================================================================
void KdTree::DeleteTree_r( KD_NODE_t *rootNode )
{
	if ( rootNode->LEFT_CHILD_ptr != nullptr )
		DeleteTree_r ( rootNode->LEFT_CHILD_ptr );
	if ( rootNode->RIGHT_CHILD_ptr != nullptr )
		DeleteTree_r ( rootNode->RIGHT_CHILD_ptr );
	delete rootNode;
}

//=============================================================================
// GetUpperRootCentroidBound
//	This method returns a XMFLOAT3 data type with the upper bounds of the
//	complete triangle list based on a triangles centroid.
//=============================================================================
XMFLOAT3 KdTree::GetUpperRootCentroidBound( vector<TriangleAccessor *> triangleList )
{
	XMFLOAT3 BB = XMFLOAT3(-FLT_MAX, -FLT_MAX, -FLT_MAX);
	for ( int i = 0; i < triangleList.size(); ++i )
	{
		if ( BB.x < triangleList.at(i)->GetCentroid().x )
			 BB.x = triangleList.at(i)->GetCentroid().x;
		if ( BB.y < triangleList.at(i)->GetCentroid().y )
			 BB.y = triangleList.at(i)->GetCentroid().y;
		if ( BB.z < triangleList.at(i)->GetCentroid().z )
			 BB.z = triangleList.at(i)->GetCentroid().z;
	}
	return BB;
}

//=============================================================================
// GetLowerRootCentroidBound
//	This method returns a XMFLOAT3 data type with the lower bounds of the
//	complete triangle list based on a triangles centroid.
//=============================================================================
XMFLOAT3 KdTree::GetLowerRootCentroidBound( vector<TriangleAccessor *> triangleList )
{
	XMFLOAT3 BB = XMFLOAT3(FLT_MAX, FLT_MAX, FLT_MAX);
	for ( int i = 0; i < triangleList.size(); ++i )
	{
		if ( BB.x > triangleList.at(i)->GetCentroid().x )
			 BB.x = triangleList.at(i)->GetCentroid().x;
		if ( BB.y > triangleList.at(i)->GetCentroid().y )
			 BB.y = triangleList.at(i)->GetCentroid().y;
		if ( BB.z > triangleList.at(i)->GetCentroid().z )
			 BB.z = triangleList.at(i)->GetCentroid().z;
	}
	return BB;
}

//=============================================================================
// GetSubtreeBounds
//	This method calculates and sets the min and max bounds of the new subtree
//	based on the previous subtree and previous splitting plane
//=============================================================================
void KdTree::GetSubtreeBounds ( KD_NODE_t *current_node, KD_NODE_t *prev_node )
{
	if ( current_node->BRANCH_WAY == KD_NODE_t::Branch::left )
	{
		// Left branch
		if ( prev_node->DIVIDES_BY == KD_NODE_t::Dimension::x )
		{
			current_node->BB_MIN = prev_node->BB_MIN;
			current_node->BB_MAX = prev_node->SPLITTING_PLANE[2];
		}
		else if ( prev_node->DIVIDES_BY == KD_NODE_t::Dimension::y )
		{
			current_node->BB_MIN = prev_node->BB_MIN;
			current_node->BB_MAX = prev_node->SPLITTING_PLANE[2];
		}
		else
		{
			current_node->BB_MIN = prev_node->BB_MIN;
			current_node->BB_MAX = prev_node->SPLITTING_PLANE[2];
		}
	}
	else
	{
		// Right branch
		if ( current_node->DIVIDES_BY == KD_NODE_t::Dimension::x )
		{
			current_node->BB_MIN = prev_node->SPLITTING_PLANE[0];
			current_node->BB_MAX = prev_node->BB_MAX;
		}
		else if ( prev_node->DIVIDES_BY == KD_NODE_t::Dimension::y )
		{
			current_node->BB_MIN = prev_node->SPLITTING_PLANE[0];
			current_node->BB_MAX = prev_node->BB_MAX;
		}
		else
		{
			current_node->BB_MIN = prev_node->SPLITTING_PLANE[0];
			current_node->BB_MAX = prev_node->BB_MAX;
		}
	}
}

//=============================================================================
// ChooseSplittingDimension
//	This method returns the splitting dimension as short. The splitting
//	dimension is choosen from the longest axis in the current subtree.
//=============================================================================
short KdTree::ChooseSplittingDimension ( KD_NODE_t *node )
{
	short lookup_dimension = 0;

	if ( (node->BB_MAX.x - node->BB_MIN.x) >= (node->BB_MAX.y - node->BB_MIN.y) &&
		 (node->BB_MAX.x - node->BB_MIN.x) >= (node->BB_MAX.z - node->BB_MIN.z) )
		lookup_dimension = 0;
	else if ( (node->BB_MAX.y - node->BB_MIN.y) >= (node->BB_MAX.x - node->BB_MIN.x) &&
			  (node->BB_MAX.y - node->BB_MIN.y) >= (node->BB_MAX.z - node->BB_MIN.z) )
		lookup_dimension = 1;
	else
		lookup_dimension = 2;

	return lookup_dimension;
}

//=============================================================================
// GetSplittingPlanePoints
//	This method returns stores the four points of the splitting plane in
//	splitting_points[] 
//=============================================================================
void KdTree::GetSplittingPlanePoints ( KD_NODE_t *prev_node, int median, int split_dimension )
{
	float centX = prev_node->MEDIAN_CENTROID.x,
		  centY = prev_node->MEDIAN_CENTROID.y,
		  centZ = prev_node->MEDIAN_CENTROID.z,
		  maxX = prev_node->BB_MAX.x,
		  maxY = prev_node->BB_MAX.y,
		  maxZ = prev_node->BB_MAX.z,
		  minX = prev_node->BB_MIN.x,
		  minY = prev_node->BB_MIN.y,
		  minZ = prev_node->BB_MIN.z;

	if ( split_dimension == 0 )	// for x
	{
		prev_node->SPLITTING_PLANE[0].x = centX;
		prev_node->SPLITTING_PLANE[1].x = centX;
		prev_node->SPLITTING_PLANE[2].x = centX;
		prev_node->SPLITTING_PLANE[3].x = centX;

		prev_node->SPLITTING_PLANE[0].y = minY;
		prev_node->SPLITTING_PLANE[1].y = maxY;
		prev_node->SPLITTING_PLANE[2].y = maxY;
		prev_node->SPLITTING_PLANE[3].y = minY;

		prev_node->SPLITTING_PLANE[0].z = minZ;
		prev_node->SPLITTING_PLANE[1].z = minZ;
		prev_node->SPLITTING_PLANE[2].z = maxZ;
		prev_node->SPLITTING_PLANE[3].z = maxZ;
	}
	else if ( split_dimension == 1 )	// for y
	{
		prev_node->SPLITTING_PLANE[0].x = minX;
		prev_node->SPLITTING_PLANE[1].x = maxX;
		prev_node->SPLITTING_PLANE[2].x = maxX;
		prev_node->SPLITTING_PLANE[3].x = minX;
		
		prev_node->SPLITTING_PLANE[0].y = centY;
		prev_node->SPLITTING_PLANE[1].y = centY;
		prev_node->SPLITTING_PLANE[2].y = centY;
		prev_node->SPLITTING_PLANE[3].y = centY;
		
		prev_node->SPLITTING_PLANE[0].z = minZ;
		prev_node->SPLITTING_PLANE[1].z = minZ;
		prev_node->SPLITTING_PLANE[2].z = maxZ;
		prev_node->SPLITTING_PLANE[3].z = maxZ;
	}
	else	// for z
	{
		prev_node->SPLITTING_PLANE[0].x = minX;
		prev_node->SPLITTING_PLANE[1].x = maxX;
		prev_node->SPLITTING_PLANE[2].x = maxX;
		prev_node->SPLITTING_PLANE[3].x = minX;
		
		prev_node->SPLITTING_PLANE[0].y = minY;
		prev_node->SPLITTING_PLANE[1].y = minY;
		prev_node->SPLITTING_PLANE[2].y = maxY;
		prev_node->SPLITTING_PLANE[3].y = maxY;
		
		prev_node->SPLITTING_PLANE[0].z = centZ;
		prev_node->SPLITTING_PLANE[1].z = centZ;
		prev_node->SPLITTING_PLANE[2].z = centZ;
		prev_node->SPLITTING_PLANE[3].z = centZ;
	}
}

//=============================================================================
// VisitNodes_r
//	This method traverses the tree and looks at all the nodes in which the ray
//	is present. The first triangle the ray hits via ray triangle intersection
//	has to be returned.
//=============================================================================
TriangleAccessor* KdTree::VisitNodes_r( KD_NODE_t *node, XMFLOAT3 origin_point, XMFLOAT3 ray_vector, float tmax )
{
	if (node == nullptr)
		return nullptr;	// safety measure

	// Visit current node ...
	// which child to recurse first into? (0=near,1=far)
	float median = node->SOLE_MEDIAN_VALUE;
	float origin_point_value, ray_vector_value;
	XMFLOAT3 new_origin_point;

	// Chose values based on current dimension
	if ( node->DIVIDES_BY == KD_NODE_t::Dimension::x )
	{
			origin_point_value = origin_point.x;
			ray_vector_value = ray_vector.x;
	}
	else if ( node->DIVIDES_BY == KD_NODE_t::Dimension::y )
	{
			origin_point_value = origin_point.y;
			ray_vector_value = ray_vector.y;
	}
	else
	{
		origin_point_value = origin_point.z;
		ray_vector_value = ray_vector.z;
	}

	// Check if node is leaf
	if ( node->LEFT_CHILD_ptr == nullptr && node->RIGHT_CHILD_ptr == nullptr )
	{
		//float t = ( median - origin_point_value) / ray_vector_value;
		new_origin_point.x = origin_point.x + ( ray_vector.x * tmax );
		new_origin_point.y = origin_point.y + ( ray_vector.y * tmax );
		new_origin_point.z = origin_point.z + ( ray_vector.z * tmax );

		for ( int idx = 0; idx < node->STORED_TRI_LIST_ptr.size(); ++idx )
		{
			if ( IntersectLineTriangle( node->STORED_TRI_LIST_ptr[idx], origin_point, new_origin_point ) )
				return node->STORED_TRI_LIST_ptr[idx];	// Terminate complete method and return triangle
		}
		return nullptr;
	}

	if ( ray_vector_value == 0.0f )
	{
		// Line segment parallel to splitting plane, visit near side only
		if ( ray_vector_value < median )
			return VisitNodes_r( node->LEFT_CHILD_ptr, origin_point, ray_vector, tmax );
		else
			return VisitNodes_r( node->RIGHT_CHILD_ptr, origin_point, ray_vector, tmax );
	} 
	else
	{
		// Find t value for intersection
		float t = ( median - origin_point_value) / ray_vector_value;
		// origin_point + ( t * vec_ray )
		new_origin_point.x = origin_point.x + ( t * ray_vector.x );
		new_origin_point.y = origin_point.y + ( t * ray_vector.y );
		new_origin_point.z = origin_point.z + ( t * ray_vector.z );

		if ( 0.0f <= t && t < tmax )
		{
			if ( ray_vector_value < median )
			{
				TriangleAccessor* TA_ptr = nullptr;
				TA_ptr = VisitNodes_r( node->LEFT_CHILD_ptr, origin_point, ray_vector, tmax );
				if ( TA_ptr != nullptr )
					return TA_ptr;	// Triangle has been found. Terminate method
				else
					return VisitNodes_r( node->RIGHT_CHILD_ptr, new_origin_point, ray_vector, tmax-t );
			}
			else
			{
				TriangleAccessor* TA_ptr = nullptr;
				TA_ptr = VisitNodes_r( node->RIGHT_CHILD_ptr, origin_point, ray_vector, tmax );
				if ( TA_ptr != nullptr )
					return TA_ptr;	// Triangle has been found. Terminate method
				else
					return VisitNodes_r( node->LEFT_CHILD_ptr, new_origin_point, ray_vector, tmax-t );
			}
		}
		else
		{
			if ( ray_vector_value < median )
				return VisitNodes_r( node->LEFT_CHILD_ptr, origin_point, ray_vector, tmax );
			else
				return VisitNodes_r( node->RIGHT_CHILD_ptr, origin_point, ray_vector, tmax );
		}
	}
}

bool KdTree::IntersectLineTriangle(TriangleAccessor* triangle, XMFLOAT3 origin_point, XMFLOAT3 ray_vector)
{
	// abc = dreieck
	// pq = ray

	// Need to convert from XMFLOAT3 to XMVECTOR with the XMLoadFloat3 method
	XMVECTOR a, b, c;
	a = XMLoadFloat3(&triangle->GetRealPositions()[0]);
	b = XMLoadFloat3(&triangle->GetRealPositions()[1]);
	c = XMLoadFloat3(&triangle->GetRealPositions()[2]);

	// Convert XMFLOAT3 parameter to XMVECTOR
	XMVECTOR origin_point_XMV = XMVectorSet( origin_point.x, origin_point.y, origin_point.z, 0.0f );
	XMVECTOR ray_vector_XMV = XMVectorSet( ray_vector.x, ray_vector.y, ray_vector.z, 0.0f );

	//XMVECTOR pq = ray_vector_XMV - origin_point_XMV;
	//XMVECTOR pa = a - origin_point_XMV;
	//XMVECTOR pb = b - origin_point_XMV;
	//XMVECTOR pc = c - origin_point_XMV;

	//float u, v, w;
	//XMVECTOR crossProduct;

	//crossProduct = XMVector3Cross(pc, pb);
	//XMStoreFloat(&u, XMVector3Dot(pq, crossProduct));	// Store the calculated dot product into a float value

	//if (u < 0.0f)
	//	return false;

	//crossProduct = XMVector3Cross(pa, pc);
	//XMStoreFloat(&v, XMVector3Dot(pq, crossProduct));	// Store the calculated dot product into a float value

	//if (v < 0.0f)
	//	return false;

	//crossProduct = XMVector3Cross(pb, pa);
	//XMStoreFloat(&w, XMVector3Dot(pq, crossProduct));	// Store the calculated dot product into a float value

	//if (w < 0.0f)
	//	return false;

	XMVECTOR ab = b - a;
	XMVECTOR ac = c - a;
	XMVECTOR qp = origin_point_XMV - ray_vector_XMV;

	XMVECTOR n = XMVector3Cross(ab, ac);

	float d = 0.0f;
	XMStoreFloat(&d,  XMVector3Dot(qp, n));
	if ( d <= 0.0f )
		return false;

	XMVECTOR ap = origin_point_XMV - a;
	float t = 0.0f;
	XMStoreFloat(&t,  XMVector3Dot(ap, n));

	//if ( t < 0.0f )
	//	return false;
	//if ( t > d )
	//	return false;	// For segment. Exclude this code line for a ray test

	XMVECTOR e = XMVector3Cross(qp, ap);
	float v = 0.0f;
	XMStoreFloat(&v,  XMVector3Dot(ac, e));
	if ( v < 0.0f ||v > d )
		return false;
	
	float w = 0.0f;
	XMStoreFloat(&w,  XMVector3Dot(ab, e));
	w = -w;
	if ( w < 0.0f || ((v+w) > d) )
		return false;

	return true;
}

bool KdTree::FillLineList ( void )
{
	if ( m_LINEVERTEX_LIST.empty() )
		return false;
	m_LINELIST = new LineList(&m_LINEVERTEX_LIST[0], m_LINEVERTEX_LIST.size());
	return true;
}

LineList* KdTree::GetLineList( void )
{
	return m_LINELIST;
}
